/*
 * tarm_runR.c
 * function: pv axis linkage function + accelate function
 *  s作用：
 */

#include <stdlib.h>

// #include "tarm_timer.h"
#include "TARM_Middleware/tarm_math.h"

#include "TARM_Hardware/tarm_io.h"
#include "TARM_Hardware/tarm_limit.h"
#include "TARM_Hardware/tarm_pwm.h"
#include "TARM_Hardware/tarm_baseTimer.h"

#include "TARM_Application/tarm_init.h"
#include "TARM_Application/tarm_run.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>

// #include "hardware/closeloop/CloseLoop.h"


int handle_rMainLoop(double r,uint16_t speed_ms)
{
	uint16_t r_time,i=0;
//	if(g_TARM_RunRecord.StartRun_r == 0)
//		return false;

	//1.1、转换r为栅格中的标准值
	r = ((int)(r/g_TARM_Factor.step_p))*g_TARM_Factor.step_p;

	//2、修改rotation的值
	g_TARM_RunRecord.r_val.d = r;
	g_TARM_RunRecord.r_val.c = 0;

	//4、计算对应每步速度（每步多少ms）
	r_time = (g_TARM_Factor.step_r*speed_ms)/fabs(g_TARM_RunRecord.r_val.d);

	//5、计算重装载值、分频系数，初始化定时器
//	r_time = r_time/10;
	if(r_time<2)
		r_time = 2;

	//6、同步时间+启动定时器
	g_TARM_RunRecord.CounterTimer_r = r_time;
	
	while(1)
	{
			//1.1、判断方向
	if(g_TARM_RunRecord.r_val.d>0)
		Z_DIR_SET;
	else
		Z_DIR_RESET;
	
		
		TimePV_StartDelayCounter(g_TARM_RunRecord.CounterTimer_r*1000);
		Z_STP_RESET;
		TimePV_WaitForHalf();
		Z_STP_SET;
		TimePV_WaitForFinish();
		g_TARM_RunRecord.r_val.c += g_TARM_Factor.step_p;
		if(fabs(fabs(g_TARM_RunRecord.r_val.c) - fabs(g_TARM_RunRecord.r_val.d)) <= 0.02)
			break;
	}
//	g_TARM_RunRecord.StartRun_r = 0;
	return 1;
}
